#include "Graph.h"

#include <iostream>

#include "Vector2.h";

Graph::Graph(){}

Graph::Graph(int x, int y, int w, int h)
{
	initialize(x, y, w, h);
}


Graph::~Graph(void)
{
}

bool Graph::initialize(int x, int y, int w, int h){
	width = w, height = h;
	this->x = x, this->y = y;
	graph.resize(w*h);

	return true;
}

bool Graph::setColumn(int col, std::string& line){
	if(line.size() < height) return false;
	
	node* n;

	for(int i = 0; i < height; i++){
		n = &graph[i*width+col];
		n->nextToOccupied = false;
		n->x = (float)(col + x) + .5f;
		n->y = (float)(i + y) + .5f;
		n->isGoal = false;
		n->previous = NULL;
		n->flag = 0;
		n->cost = -1;
		n->cost2 = -1;

		if(line.at(i) == '1') n->occupied = true;
		else n->occupied = false;
	}
}

void Graph::buildEdges(){
	double sqrt2 = std::sqrt(2.0);
	int nbrs[8][2];
	node* n;
	node* g;

	for(int i = 0; i < height; i++){
		for(int j = 0; j < width; j ++){
			n = &graph[i*width+j];
			nbrs[0][0] = nbrs[1][0] = nbrs[2][0] = nbrs[3][0] = nbrs[4][0] = nbrs[5][0] = nbrs[6][0] = nbrs[7][0] =-1;
			nbrs[0][1] = nbrs[1][1] = nbrs[2][1] = nbrs[3][1] = nbrs[4][0] = nbrs[5][0] = nbrs[6][0] = nbrs[7][0] =-1;

			if(j > 0){
				nbrs[0][0] = j-1;
				nbrs[0][1] = i;
			}
			if(j < width-1){
				nbrs[1][0] = j+1;
				nbrs[1][1] = i;
			}
			if(i > 0){
				if(j != 0){
					nbrs[4][0] = j-1;
					nbrs[4][1] = i-1;
				}

				nbrs[2][0] = j;
				nbrs[2][1] = i-1;
				
				if(j < width-1){
					nbrs[5][0] = j+1;
					nbrs[5][1] = i-1;
				}
			}
			if(i != height-1){
				if(j != 0){
					nbrs[6][0] = j-1;
					nbrs[6][1] = i+1;
				}
				
				nbrs[3][0] = j;
				nbrs[3][1] = i+1;
				
				if(j < width-1){
					nbrs[7][0] = j+1;
					nbrs[7][1] = i+1;
				}
			}

			for(int k = 0; k < 8; k ++){
				if(nbrs[k][0] == -1) continue;
				g = &graph[nbrs[k][1]*width+nbrs[k][0]];

				if(g->occupied){
					n->nextToOccupied = true;
					continue;
				}
					
				if(std::abs(nbrs[k][0]-j) == 1 && std::abs(nbrs[k][1]-i) == 1){
					n->neighbors.push_back(edge::newEdge(n, g, sqrt2));
				}
				else{
					n->neighbors.push_back(edge::newEdge(n, g, 1));
				}
			}
		}
	}
}

node* Graph::getNode(Vector2 v){
	return &graph[(int)(v.y-y)*width+(int)(v.x-x)];
}

Vector2 Graph::getTopLeft(){
	return Vector2(x, y);
}


void Graph::setObstacle(Vector2 v){
	graph[(int)(v.y-y)*width+(int)(v.x-x)].occupied = true;
}

void Graph::setGoal(Vector2 v){
	graph[(int)(v.y-y)*width+(int)(v.x-x)].isGoal = true;
}

void Graph::clearFlags(){
	for(int i = 0; i < height; i ++){
		for(int j = 0; j < width; j ++){
			graph[i*width+j].isGoal = false;
			graph[i*width+j].previous = NULL;
			graph[i*width+j].flag = -1;
			graph[i*width+j].cost = -1;
			graph[i*width+j].cost2 = -1;
		}
	}
}